import sensor, image, time, pyb
from pyb import UART
from pyb import LED


THRESHOLD = (0, 20)  # 黑色阈值
# OpenMV的灰度图: 0是纯黑, 255是纯白
BALCK = 0
WITE = 255


SCREEN_W = 80
SCREEN_H = 60
ROI_W = 80
ROI_H = 50
ROI = (int(SCREEN_W/2 - ROI_W/2), int(SCREEN_H/2-ROI_H/2) + 6, ROI_W, ROI_H)

STOP_ROI_W = 5
STOP_ROI_H = 5

OFFSET = 15

STOP1_X = 20
STOP2_X = STOP1_X + OFFSET
STOP3_X = STOP2_X + OFFSET

STOP_Y = 0

STOP1_ROI = (STOP1_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)
STOP2_ROI = (STOP2_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)
STOP3_ROI = (STOP3_X, STOP_Y, STOP_ROI_W, STOP_ROI_H)

STOP11_ROI = (STOP1_X, STOP_Y + 5, STOP_ROI_W, STOP_ROI_H)
STOP22_ROI = (STOP2_X, STOP_Y + 5, STOP_ROI_W, STOP_ROI_H)
STOP33_ROI = (STOP3_X, STOP_Y + 5, STOP_ROI_W, STOP_ROI_H)

uart = UART(3, 115200)
current_time = 0
begin_stop_judge = False


def calc_mode(img, roi=None):
    if roi is None:
        roi_x, roi_y = 0, 0
        roi_w = img.width()
        roi_h = img.height()
    else:
        roi_x, roi_y, roi_w, roi_h = roi[:]

    pix_dic = {}

    for row in range(roi_x, roi_x + roi_w + 1):
        for col in range(roi_y, roi_y + roi_h + 1):
            pix_val = img.get_pixel(row, col)
            if pix_val in pix_dic:
                pix_dic[pix_val] += 1
            else:
                pix_dic[pix_val] = 0


    max_val = -1
    res_mode = -1
    for key, val in pix_dic.items():
        if val > max_val:
            max_val = val
            res_mode = key

    return res_mode


#LED(1).on()
#LED(2).on()
#LED(3).on()

sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)

sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
sensor.set_pixformat(sensor.GRAYSCALE)
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time=2000)  # 为了让摄像头稳定, 等待2s再让摄像头开始工作

time_start = pyb.millis()  # 开始计时
while(True):
    current_time = pyb.elapsed_millis(time_start)
    print("now time: %d" % int(current_time/1000))

    if current_time >= 5000:
        begin_stop_judge = True

    img = sensor.snapshot()
    img = img.binary([THRESHOLD], invert=True)  # 阈值内的像素变为0(黑)

    roi_img = img.copy(roi=ROI, copy_to_fb=False)
    stop_img = img.copy(copy_to_fb=False)

    img.draw_rectangle(ROI, color = BALCK)

    # 判停框
    img.draw_rectangle(STOP1_ROI, color = WITE)
    img.draw_rectangle(STOP2_ROI, color = WITE)
    img.draw_rectangle(STOP3_ROI, color = WITE)
    img.draw_rectangle(STOP11_ROI, color = WITE)
    img.draw_rectangle(STOP22_ROI, color = WITE)
    img.draw_rectangle(STOP33_ROI, color = WITE)

    # 判停
    if begin_stop_judge:  # 到达指定秒数后才进行停止判断
        mode1 = calc_mode(stop_img, STOP1_ROI)
        mode2 = calc_mode(stop_img, STOP2_ROI)
        mode3 = calc_mode(stop_img, STOP3_ROI)
        mode11 = calc_mode(stop_img, STOP11_ROI)
        mode22 = calc_mode(stop_img, STOP22_ROI)
        mode33 = calc_mode(stop_img, STOP33_ROI)
        if (mode1 == 0 and mode22 == 0 or mode2 == 0 and mode11 == 0) or (mode2 == 0 and mode22 == 0 and mode33 == 0 and mode3 == 0):
            # print("e")
            uart.write("e")
            break


    line = roi_img.get_regression([(0,0)], robust = True)
    if (line):

        # rho_err = abs(line.rho())-roi_img.width()/2

        if line.theta()>90:
            theta_err = line.theta()-180
        else:
            theta_err = line.theta()

        # 画线性回归的线
        line_list = list(line.line())
        offset_x = int(SCREEN_W / 2 - ROI_W/2)
        offset_y = int(SCREEN_H / 2 - ROI_H/2)
        line_list[0] += offset_x;
        line_list[1] += offset_y;
        line_list[2] += offset_x;
        line_list[3] += offset_y;
        img.draw_line(tuple(line_list), color = BALCK)

        # print(rho_err,line.magnitude())

        if line.magnitude()>8:  # 线性回归效果好
            if theta_err > 20:  # 右转
                print("b")
                uart.write("b")
            elif theta_err > 10:
                print("a")
                uart.write("a")
            elif theta_err < -20:  # 左转
                print("d")
                uart.write("d");
            elif theta_err < -10:
                print("c")
                uart.write("c")
            else:
                print("f")
                uart.write("f")
        else:
            print("n")
            uart.write("n")
    else:
        print("x")
        uart.write("x")
